import cv2
# import numpy as np
import math
# import matplotlib
from pylab import *
from numpy import random
# from mpl_toolkits.mplot3d import Axes3D
# from matplotlib import cm
import matplotlib.pyplot as plt
# from mpl_toolkits.mplot3d import Axes3D


np.set_printoptions(suppress=True)

#三公共点坐标系转换算法
def calPoseFrom3Points(Oab, Pxb, Pyb):
    '''
    作者：xyq；
    日期：2022.1.18；
    功能：已知坐标系a的原点、x轴正半轴上任一点和y轴正半轴上任一点在坐标系b下的坐标，
        求解坐标系a到坐标系b的旋转矩阵R和平移矩阵T；
    输入：坐标系a的原点在坐标系b下的坐标:Oab(x1,y1,z1);
        坐标系a的x轴正半轴上任一点在坐标系b下的坐标:Pxb(x2,y2,z2);
        坐标系a的y轴正半轴上任一点在坐标系b下的坐标:Pyb(x3,y3,z3);
    输出：坐标系n到坐标系s的旋转矩阵Rns，输出格式为矩阵;
    DEMO：
            import geomeas as gm
            import numpy as np

            Oab = np.array([-37.84381632, 152.36389864, 41.68600167])
            Pxb = np.array([-19.59820338, 139.58818292, 45.55380309])
            Pyb = np.array([-38.23270656, 157.3130709, 59.86810327])

            print(gm.Pose().calPoseFrom3Points(Oab, Pxb, Pyb))
    '''
    x = (Pxb - Oab) / np.linalg.norm(Pxb - Oab)
    y = (Pyb - Oab) / np.linalg.norm(Pyb - Oab)
    z = np.cross(x, y)
    length = np.linalg.norm(z)
    z = z / length
    Rab = np.matrix([x, y, z]).transpose()
    Tab = np.matrix(Oab).transpose()
    return Rab, Tab

#根据旋转矩阵求解欧拉角
def calAngleFromRotation(R):
    # 绕Y转roll
    roll = np.arctan2(R[2, 1], R[2, 2]) * 180 / np.pi

    # 绕X转pitch
    pitch = np.arctan2(-R[2, 0], (R[0, 0] ** 2 + R[1, 0] ** 2) ** 0.5) * 180 / np.pi

    # 绕Z转yaw
    yaw = np.arctan2(R[1, 0], R[0, 0]) * 180 / np.pi
    return roll, pitch, yaw

# 反算欧拉角,输出弧度制需要进行转换为角度制
def eulerAngles2rotationMat(theta, format='degree'):
    """
    Calculates Rotation Matrix given euler angles.
    :param theta: 1-by-3 list [rx, ry, rz] angle in degree
    :return:
    RPY角，是ZYX欧拉角，依次 绕定轴XYZ转动[rx, ry, rz]
    """
    if format != 'degree':
        theta = [i * math.pi / 180.0 for i in theta]

    R_x = np.array([[1, 0, 0],
                    [0, math.cos(theta[0]), -math.sin(theta[0])],
                    [0, math.sin(theta[0]), math.cos(theta[0])]
                    ])

    R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
                    [0, 1, 0],
                    [-math.sin(theta[1]), 0, math.cos(theta[1])]
                    ])

    R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
                    [math.sin(theta[2]), math.cos(theta[2]), 0],
                    [0, 0, 1]
                    ])
    R = np.dot(R_z, np.dot(R_y, R_x))
    return R

def calPointFrom2Angles(angle_hz, angle_v):
    '''
    作者:ljx;
    日期:2022.3.29;
    功能:已知一条直线经过坐标系原点,并知其相对于坐标系的水平角与垂直角,求该直线的单位方向向量;
    输入:直线在右手坐标系下的水平角angle_hz和垂直角angle_v
       （水平角为指示激光在水平面XOY的投影线与y轴的夹角,垂直角为指示激光直线与水平面XOY的夹角）;
    输出:该直线以原点为起点的单位方向向量;
    DEMO:   import GeoMeas as gm

            angle_hz = 30.36447776
            angle_v  = 86.83401453
            print(calVectorFrom2Angles(angle_hz, angle_v))
    '''
    hz = math.radians(angle_hz)
    v = math.radians(angle_v)

    x = math.cos(v) * math.cos(hz)
    z = -math.cos(v) * math.sin(hz)
    y = math.sin(v)

    Vector = np.array([x, y, z])

    return Vector



#######################误差仿真###############################
if __name__ == '__main__':

    # 数理统计
    a_meanlist = []
    a_stdlist = []
    b_meanlist = []
    b_stdlist = []

    # 0.无误差
    a0list = []
    b0list = []

    # 1.相机标定误差
    a1list = []
    b1list = []

    # 2.像素坐标提取误差
    a2list = []
    b2list = []

    # 3.标定误差
    a3list = []
    b3list = []

    # 4.总误差
    a4list = []
    b4list = []


    ############################## 0.无误差 ##############################

    for i in range(500):

        mtx_l = np.array([[8 / 0.0053, 0, 640, 0],
                          [0, 8 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[8 / 0.0053, 0, 640, 0],
                          [0, 8 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[-145],
                         [0],
                         [0]])

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        PL = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        PR = np.dot(mtx_r, _temp_R_lr)

        Tb0_l = np.array([3.55550593 / 0.0053, 2.65832473 / 0.0053])  # 基准工位处光电标靶
        Tb0_r = np.array([6.39255778 / 0.0053, 2.65832473 / 0.0053])
        Tb1_l = np.array([3.14294457 / 0.0053, 2.65950158 / 0.0053])
        Tb1_r = np.array([5.98056471 / 0.0053, 2.65950158 / 0.0053])
        Tb2_l = np.array([3.55547749 / 0.0053, 2.32865551 / 0.0053])
        Tb2_r = np.array([6.40594319 / 0.0053, 2.32865551 / 0.0053])

        Tk0_l = np.array([1.95960674 / 0.0053, 1.67420817 / 0.0053])  # 待校准工位处光电标靶
        Tk0_r = np.array([3.61379518 / 0.0053, 1.67420817 / 0.0053])
        Tk1_l = np.array([1.71908251 / 0.0053, 1.67420817 / 0.0053])
        Tk1_r = np.array([3.37327096 / 0.0053, 1.67420817 / 0.0053])
        Tk2_l = np.array([1.95259578 / 0.0053, 1.47976580 / 0.0053])
        Tk2_r = np.array([3.61488078 / 0.0053, 1.47976580 / 0.0053])

        Tb0 = np.array(cv2.triangulatePoints(PL, PR, Tb0_l, Tb0_r)).T
        wp_Tb0 = Tb0[0][:-1] / np.max(Tb0[0][-1])
        Tb1 = np.array(cv2.triangulatePoints(PL, PR, Tb1_l,Tb1_r)).T
        wp_Tb1 = Tb1[0][:-1] / np.max(Tb1[0][-1])
        Tb2 = np.array(cv2.triangulatePoints(PL, PR, Tb2_l, Tb2_r)).T
        wp_Tb2 = Tb2[0][:-1] / np.max(Tb2[0][-1])
        Tk0 = np.array(cv2.triangulatePoints(PL, PR, Tk0_l, Tk0_r)).T
        wp_Tk0 = Tk0[0][:-1] / np.max(Tk0[0][-1])
        Tk1 = np.array(cv2.triangulatePoints(PL, PR, Tk1_l, Tk1_r)).T
        wp_Tk1 = Tk1[0][:-1] / np.max(Tk1[0][-1])
        Tk2 = np.array(cv2.triangulatePoints(PL, PR, Tk2_l, Tk2_r)).T
        wp_Tk2 = Tk2[0][:-1] / np.max(Tk2[0][-1])

        # 光电标靶坐标系与世界坐标系间旋转矩阵
        Rw_tb = calPoseFrom3Points(wp_Tb0, wp_Tb1, wp_Tb2)[0]  # 基准标靶
        Rw_tk = calPoseFrom3Points(wp_Tk0, wp_Tk1, wp_Tk2)[0]  # 待测标靶

        Rtb_tk = np.dot(np.transpose(Rw_tb), Rw_tk)  # 待校准标靶系与基准标靶系间旋转矩阵

        angle_hz = 87.06294733  #待校准工位轴线角度
        angle_v = 25.78054998
        hz_b = 74.87478168      #基准工位轴线角度
        v_b = 40.70786725

        PP = calPointFrom2Angles(angle_hz, angle_v)
        P = np.dot(PP, Rtb_tk.T).T

        x = P[0]
        y = P[1]
        z = P[2]

        hz_new = math.atan2(-z, x) * 180 / math.pi
        v_new = math.atan2(y, math.sqrt(x ** 2 + z ** 2)) * 180 / math.pi

        hz = hz_b - hz_new
        v = v_b - v_new

        a0list.append(hz)
        b0list.append(v)
    print('a0', a0list)
    print('b0', b0list)

    ######################################### 1.添加相机标定误差 #########################################

    for i in range(500):
        l_fx = random.uniform(0, 1)
        l_fy = random.uniform(0, 1)
        l_u0 = random.uniform(-1, 1)
        l_v0 = random.uniform(-1, 1)
        mtx_l12 = random.uniform(-0.1, 0.1)

        r_fx = random.uniform(0, 1)
        r_fy = random.uniform(0, 1)
        r_u0 = random.uniform(-1, 1)
        r_v0 = random.uniform(-1, 1)
        mtx_r12 = random.uniform(-0.1, 0.1)

        R = random.uniform(0, 0.005)
        P = random.uniform(0, 0.005)
        Y = random.uniform(0, 0.005)
        T_e = random.uniform(0, 0.5, size=(3, 1))

        mtx_l = np.array([[8 / 0.0053 + l_fx, mtx_l12, 640 + l_u0, 0],
                          [0, 8 / 0.0053 + l_fy, 512 + l_v0, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[8 / 0.0053 + r_fx, mtx_r12, 640 + r_u0, 0],
                          [0, 8 / 0.0053 + r_fy, 512 + r_v0, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[-145],
                         [0],
                         [0]])
        roll, pitch, yaw = calAngleFromRotation(R_lr)  # 弧度制
        roll0 = math.degrees(roll)
        pitch0 = math.degrees(pitch)
        yaw0 = math.degrees(yaw)
        roll_c = roll0 + R
        pitch_c = pitch0 + P
        yaw_c = yaw0 + Y
        R_lr = eulerAngles2rotationMat([roll_c * np.pi / 180, pitch_c * np.pi / 180, yaw_c * np.pi / 180])
        T_lr = T_lr + T_e

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        PL = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        PR = np.dot(mtx_r, _temp_R_lr)

        Tb0_l = np.array([3.55550593 / 0.0053, 2.65832473 / 0.0053])  # 基准工位处光电标靶
        Tb0_r = np.array([6.39255778 / 0.0053, 2.65832473 / 0.0053])
        Tb1_l = np.array([3.14294457 / 0.0053, 2.65950158 / 0.0053])
        Tb1_r = np.array([5.98056471 / 0.0053, 2.65950158 / 0.0053])
        Tb2_l = np.array([3.55547749 / 0.0053, 2.32865551 / 0.0053])
        Tb2_r = np.array([6.40594319 / 0.0053, 2.32865551 / 0.0053])

        Tk0_l = np.array([1.95960674 / 0.0053, 1.67420817 / 0.0053])  # 待校准工位处光电标靶
        Tk0_r = np.array([3.61379518 / 0.0053, 1.67420817 / 0.0053])
        Tk1_l = np.array([1.71908251 / 0.0053, 1.67420817 / 0.0053])
        Tk1_r = np.array([3.37327096 / 0.0053, 1.67420817 / 0.0053])
        Tk2_l = np.array([1.95259578 / 0.0053, 1.47976580 / 0.0053])
        Tk2_r = np.array([3.61488078 / 0.0053, 1.47976580 / 0.0053])

        Tb0 = np.array(cv2.triangulatePoints(PL, PR, Tb0_l, Tb0_r)).T
        wp_Tb0 = Tb0[0][:-1] / np.max(Tb0[0][-1])
        Tb1 = np.array(cv2.triangulatePoints(PL, PR, Tb1_l, Tb1_r)).T
        wp_Tb1 = Tb1[0][:-1] / np.max(Tb1[0][-1])
        Tb2 = np.array(cv2.triangulatePoints(PL, PR, Tb2_l, Tb2_r)).T
        wp_Tb2 = Tb2[0][:-1] / np.max(Tb2[0][-1])
        Tk0 = np.array(cv2.triangulatePoints(PL, PR, Tk0_l, Tk0_r)).T
        wp_Tk0 = Tk0[0][:-1] / np.max(Tk0[0][-1])
        Tk1 = np.array(cv2.triangulatePoints(PL, PR, Tk1_l, Tk1_r)).T
        wp_Tk1 = Tk1[0][:-1] / np.max(Tk1[0][-1])
        Tk2 = np.array(cv2.triangulatePoints(PL, PR, Tk2_l, Tk2_r)).T
        wp_Tk2 = Tk2[0][:-1] / np.max(Tk2[0][-1])

        # 光电标靶坐标系与世界坐标系间旋转矩阵
        Rw_tb = calPoseFrom3Points(wp_Tb0, wp_Tb1, wp_Tb2)[0]  # 基准标靶
        Rw_tk = calPoseFrom3Points(wp_Tk0, wp_Tk1, wp_Tk2)[0]  # 待测标靶

        Rtb_tk = np.dot(np.transpose(Rw_tb), Rw_tk)  # 待校准标靶系与基准标靶系间旋转矩阵

        angle_hz = 87.06294733
        angle_v = 25.78054998
        hz_b = 74.87478168
        v_b = 40.70786725

        PP = calPointFrom2Angles(angle_hz, angle_v)
        P = np.dot(PP, Rtb_tk.T).T

        x = P[0]
        y = P[1]
        z = P[2]

        hz_new = math.atan2(-z, x) * 180 / math.pi
        v_new = math.atan2(y, math.sqrt(x ** 2 + z ** 2)) * 180 / math.pi

        hz = hz_b - hz_new
        v = v_b - v_new

        a1list.append(hz)
        b1list.append(v)

    print('a1', a1list)
    print('b1', b1list)



    ############################# 2.添加像素坐标提取误差 ##############################

    for i in range(500):
        a = random.uniform(0, 0.005, size=(1, 2))
        b = random.uniform(0, 0.005, size=(1, 2))
        c = random.uniform(0, 0.005, size=(1, 2))
        d = random.uniform(0, 0.005, size=(1, 2))
        e = random.uniform(0, 0.005, size=(1, 2))
        f = random.uniform(0, 0.005, size=(1, 2))
        g = random.uniform(0, 0.005, size=(1, 2))
        h = random.uniform(0, 0.005, size=(1, 2))
        i = random.uniform(0, 0.005, size=(1, 2))
        j = random.uniform(0, 0.005, size=(1, 2))
        k = random.uniform(0, 0.005, size=(1, 2))
        l = random.uniform(0, 0.005, size=(1, 2))

        mtx_l = np.array([[8 / 0.0053, 0, 640, 0],
                          [0, 8 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[8 / 0.0053, 0, 640, 0],
                          [0, 8 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[-145],
                         [0],
                         [0]])

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        PL = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        PR = np.dot(mtx_r, _temp_R_lr)

        Tb0_l = np.array([3.55550593 / 0.0053, 2.65832473 / 0.0053])  # 基准工位处光电标靶
        Tb0_r = np.array([6.39255778 / 0.0053, 2.65832473 / 0.0053])
        Tb1_l = np.array([3.14294457 / 0.0053, 2.65950158 / 0.0053])
        Tb1_r = np.array([5.98056471 / 0.0053, 2.65950158 / 0.0053])
        Tb2_l = np.array([3.55547749 / 0.0053, 2.32865551 / 0.0053])
        Tb2_r = np.array([6.40594319 / 0.0053, 2.32865551 / 0.0053])

        Tk0_l = np.array([1.95960674 / 0.0053, 1.67420817 / 0.0053])  # 待校准工位处光电标靶
        Tk0_r = np.array([3.61379518 / 0.0053, 1.67420817 / 0.0053])
        Tk1_l = np.array([1.71908251 / 0.0053, 1.67420817 / 0.0053])
        Tk1_r = np.array([3.37327096 / 0.0053, 1.67420817 / 0.0053])
        Tk2_l = np.array([1.95259578 / 0.0053, 1.47976580 / 0.0053])
        Tk2_r = np.array([3.61488078 / 0.0053, 1.47976580 / 0.0053])

        Tb0_l = Tb0_l + a[0, :]
        Tb0_r = Tb0_r + b[0, :]
        Tb1_l = Tb1_l + c[0, :]
        Tb1_r = Tb1_r + d[0, :]
        Tb2_l = Tb2_l + e[0, :]
        Tb2_r = Tb2_r + f[0, :]
        Tk0_l = Tk0_l + g[0, :]
        Tk0_r = Tk0_r + h[0, :]
        Tk1_l = Tk1_l + i[0, :]
        Tk1_r = Tk1_r + j[0, :]
        Tk2_l = Tk2_l + k[0, :]
        Tk2_r = Tk2_r + l[0, :]

        Tb0 = np.array(cv2.triangulatePoints(PL, PR, Tb0_l, Tb0_r)).T
        wp_Tb0 = Tb0[0][:-1] / np.max(Tb0[0][-1])
        Tb1 = np.array(cv2.triangulatePoints(PL, PR, Tb1_l, Tb1_r)).T
        wp_Tb1 = Tb1[0][:-1] / np.max(Tb1[0][-1])
        Tb2 = np.array(cv2.triangulatePoints(PL, PR, Tb2_l, Tb2_r)).T
        wp_Tb2 = Tb2[0][:-1] / np.max(Tb2[0][-1])
        Tk0 = np.array(cv2.triangulatePoints(PL, PR, Tk0_l, Tk0_r)).T
        wp_Tk0 = Tk0[0][:-1] / np.max(Tk0[0][-1])
        Tk1 = np.array(cv2.triangulatePoints(PL, PR, Tk1_l, Tk1_r)).T
        wp_Tk1 = Tk1[0][:-1] / np.max(Tk1[0][-1])
        Tk2 = np.array(cv2.triangulatePoints(PL, PR, Tk2_l, Tk2_r)).T
        wp_Tk2 = Tk2[0][:-1] / np.max(Tk2[0][-1])

        # 光电标靶坐标系与世界坐标系间旋转矩阵
        Rw_tb = calPoseFrom3Points(wp_Tb0, wp_Tb1, wp_Tb2)[0]  # 基准标靶
        Rw_tk = calPoseFrom3Points(wp_Tk0, wp_Tk1, wp_Tk2)[0]  # 待测标靶

        Rtb_tk = np.dot(np.transpose(Rw_tb), Rw_tk)  # 待校准标靶系与基准标靶系间旋转矩阵

        angle_hz = 87.06294733
        angle_v = 25.78054998
        hz_b = 74.87478168
        v_b = 40.70786725

        PP = calPointFrom2Angles(angle_hz, angle_v)
        P = np.dot(PP, Rtb_tk.T).T

        x = P[0]
        y = P[1]
        z = P[2]

        hz_new = math.atan2(-z, x) * 180 / math.pi
        v_new = math.atan2(y, math.sqrt(x ** 2 + z ** 2)) * 180 / math.pi

        hz = hz_b - hz_new
        v = v_b - v_new

        a2list.append(hz)
        b2list.append(v)

    print('a2', a2list)
    print('b2', b2list)


    ######################################### 3.增加标定误差（即为光电标靶与转接架轴线标定误差，可用欧拉角表示） #########################################

    for i in range(500):
        hz1 = random.uniform(-0.001, 0.001)
        v1 = random.uniform(-0.001, 0.001)
        hz2 = random.uniform(-0.001, 0.001)
        v2 = random.uniform(-0.001, 0.001)

        mtx_l = np.array([[8 / 0.0053, 0, 640, 0],
                          [0, 8 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])
        mtx_r = np.array([[8 / 0.0053, 0, 640, 0],
                          [0, 8 / 0.0053, 512, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[-145],
                         [0],
                         [0]])

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        PL = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        PR = np.dot(mtx_r, _temp_R_lr)

        Tb0_l = np.array([3.55550593 / 0.0053, 2.65832473 / 0.0053])  # 基准工位处光电标靶
        Tb0_r = np.array([6.39255778 / 0.0053, 2.65832473 / 0.0053])
        Tb1_l = np.array([3.14294457 / 0.0053, 2.65950158 / 0.0053])
        Tb1_r = np.array([5.98056471 / 0.0053, 2.65950158 / 0.0053])
        Tb2_l = np.array([3.55547749 / 0.0053, 2.32865551 / 0.0053])
        Tb2_r = np.array([6.40594319 / 0.0053, 2.32865551 / 0.0053])

        Tk0_l = np.array([1.95960674 / 0.0053, 1.67420817 / 0.0053])  # 待校准工位处光电标靶
        Tk0_r = np.array([3.61379518 / 0.0053, 1.67420817 / 0.0053])
        Tk1_l = np.array([1.71908251 / 0.0053, 1.67420817 / 0.0053])
        Tk1_r = np.array([3.37327096 / 0.0053, 1.67420817 / 0.0053])
        Tk2_l = np.array([1.95259578 / 0.0053, 1.47976580 / 0.0053])
        Tk2_r = np.array([3.61488078 / 0.0053, 1.47976580 / 0.0053])

        Tb0 = np.array(cv2.triangulatePoints(PL, PR, Tb0_l, Tb0_r)).T
        wp_Tb0 = Tb0[0][:-1] / np.max(Tb0[0][-1])
        Tb1 = np.array(cv2.triangulatePoints(PL, PR, Tb1_l, Tb1_r)).T
        wp_Tb1 = Tb1[0][:-1] / np.max(Tb1[0][-1])
        Tb2 = np.array(cv2.triangulatePoints(PL, PR, Tb2_l, Tb2_r)).T
        wp_Tb2 = Tb2[0][:-1] / np.max(Tb2[0][-1])
        Tk0 = np.array(cv2.triangulatePoints(PL, PR, Tk0_l, Tk0_r)).T
        wp_Tk0 = Tk0[0][:-1] / np.max(Tk0[0][-1])
        Tk1 = np.array(cv2.triangulatePoints(PL, PR, Tk1_l, Tk1_r)).T
        wp_Tk1 = Tk1[0][:-1] / np.max(Tk1[0][-1])
        Tk2 = np.array(cv2.triangulatePoints(PL, PR, Tk2_l, Tk2_r)).T
        wp_Tk2 = Tk2[0][:-1] / np.max(Tk2[0][-1])

        # 光电标靶坐标系与世界坐标系间旋转矩阵
        Rw_tb = calPoseFrom3Points(wp_Tb0, wp_Tb1, wp_Tb2)[0]  # 基准标靶
        Rw_tk = calPoseFrom3Points(wp_Tk0, wp_Tk1, wp_Tk2)[0]  # 待测标靶

        Rtb_tk = np.dot(np.transpose(Rw_tb), Rw_tk)  # 待校准标靶系与基准标靶系间旋转矩阵

        angle_hz = 87.06294733
        angle_v = 25.78054998
        hz_b = 74.87478168
        v_b = 40.70786725

        angle_hz = 87.06294733 + hz1
        angle_v = 25.78054998 + v1
        hz_b = 74.87478168 + hz2
        v_b = 40.70786725 + v2

        PP = calPointFrom2Angles(angle_hz, angle_v)
        P = np.dot(PP, Rtb_tk.T).T

        x = P[0]
        y = P[1]
        z = P[2]

        hz_new = math.atan2(-z, x) * 180 / math.pi
        v_new = math.atan2(y, math.sqrt(x ** 2 + z ** 2)) * 180 / math.pi

        hz = hz_b - hz_new
        v = v_b - v_new

        a3list.append(hz)
        b3list.append(v)

    print('a3', a3list)
    print('b3', b3list)


    ######################################### 4.全误差 #########################################

    for i in range(500):
        l_fx = random.uniform(0, 1)
        l_fy = random.uniform(0, 1)
        l_u0 = random.uniform(-1, 1)
        l_v0 = random.uniform(-1, 1)
        mtx_l12 = random.uniform(-0.1, 0.1)

        r_fx = random.uniform(0, 1)
        r_fy = random.uniform(0, 1)
        r_u0 = random.uniform(-1, 1)
        r_v0 = random.uniform(-1, 1)
        mtx_r12 = random.uniform(-0.1, 0.1)

        R = random.uniform(0, 0.005)
        P = random.uniform(0, 0.005)
        Y = random.uniform(0, 0.005)
        T_e = random.uniform(0, 0.05, size=(3, 1))

        a1 = random.uniform(0, 0.005, size=(1, 2))
        b1 = random.uniform(0, 0.005, size=(1, 2))
        c1 = random.uniform(0, 0.005, size=(1, 2))
        d1 = random.uniform(0, 0.005, size=(1, 2))
        e1 = random.uniform(0, 0.005, size=(1, 2))
        f1 = random.uniform(0, 0.005, size=(1, 2))
        g1 = random.uniform(0, 0.005, size=(1, 2))
        h1 = random.uniform(0, 0.005, size=(1, 2))
        i1 = random.uniform(0, 0.005, size=(1, 2))
        j1 = random.uniform(0, 0.005, size=(1, 2))
        k1 = random.uniform(0, 0.005, size=(1, 2))
        l1 = random.uniform(0, 0.005, size=(1, 2))

        hz1 = random.uniform(-0.001, 0.001)
        v1 = random.uniform(-0.001, 0.001)
        hz2 = random.uniform(-0.001, 0.001)
        v2 = random.uniform(-0.001, 0.001)


        # 误差项1.相机标定误差
        mtx_l = np.array([[8 / 0.0053 + l_fx, mtx_l12, 640 + l_u0, 0],
                          [0, 8 / 0.0053 + l_fy, 512 + l_v0, 0],
                          [0, 0, 1, 0]])

        mtx_r = np.array([[8 / 0.0053 + r_fx, mtx_r12, 640 + r_u0, 0],
                          [0, 8 / 0.0053 + r_fy, 512 + r_v0, 0],
                          [0, 0, 1, 0]])

        R_lr = np.array([[1, 0, 0],
                         [0, 1, 0],
                         [0, 0, 1]])
        T_lr = np.array([[-145],
                         [0],
                         [0]])

        roll, pitch, yaw = calAngleFromRotation(R_lr)  # 弧度制
        roll0 = math.degrees(roll)
        pitch0 = math.degrees(pitch)
        yaw0 = math.degrees(yaw)
        roll_c = roll0 + R
        pitch_c = pitch0 + P
        yaw_c = yaw0 + Y
        R_lr = eulerAngles2rotationMat([roll_c * np.pi / 180, pitch_c * np.pi / 180, yaw_c * np.pi / 180])
        T_lr = T_lr + T_e

        cameraMatrixL = mtx_l[:, 0:3]
        cameraMatrixR = mtx_r[:, 0:3]

        # 左相机到左相机的投影矩阵
        R_ll = ([[1, 0, 0],
                 [0, 1, 0],
                 [0, 0, 1]])
        T_ll = ([[0], [0], [0]])

        # 内外参相乘
        temp_R_ll = np.append(R_ll, T_ll, axis=1)
        _temp_R_ll = np.row_stack((temp_R_ll, [0, 0, 0, 1]))
        PL = np.dot(mtx_l, _temp_R_ll)

        # 右相机到左相机的投影矩阵
        temp_R_lr = np.append(R_lr, T_lr, axis=1)
        _temp_R_lr = np.row_stack((temp_R_lr, [0, 0, 0, 1]))
        PR = np.dot(mtx_r, _temp_R_lr)

        Tb0_l = np.array([3.55550593 / 0.0053, 2.65832473 / 0.0053])  # 基准工位处光电标靶
        Tb0_r = np.array([6.39255778 / 0.0053, 2.65832473 / 0.0053])
        Tb1_l = np.array([3.14294457 / 0.0053, 2.65950158 / 0.0053])
        Tb1_r = np.array([5.98056471 / 0.0053, 2.65950158 / 0.0053])
        Tb2_l = np.array([3.55547749 / 0.0053, 2.32865551 / 0.0053])
        Tb2_r = np.array([6.40594319 / 0.0053, 2.32865551 / 0.0053])

        Tk0_l = np.array([1.95960674 / 0.0053, 1.67420817 / 0.0053])  # 待校准工位处光电标靶
        Tk0_r = np.array([3.61379518 / 0.0053, 1.67420817 / 0.0053])
        Tk1_l = np.array([1.71908251 / 0.0053, 1.67420817 / 0.0053])
        Tk1_r = np.array([3.37327096 / 0.0053, 1.67420817 / 0.0053])
        Tk2_l = np.array([1.95259578 / 0.0053, 1.47976580 / 0.0053])
        Tk2_r = np.array([3.61488078 / 0.0053, 1.47976580 / 0.0053])

        #误差项2.像素坐标提取误差
        Tb0_l = Tb0_l + a1[0, :]
        Tb0_r = Tb0_r + b1[0, :]
        Tb1_l = Tb1_l + c1[0, :]
        Tb1_r = Tb1_r + d1[0, :]
        Tb2_l = Tb2_l + e1[0, :]
        Tb2_r = Tb2_r + f1[0, :]
        Tk0_l = Tk0_l + g1[0, :]
        Tk0_r = Tk0_r + h1[0, :]
        Tk1_l = Tk1_l + i1[0, :]
        Tk1_r = Tk1_r + j1[0, :]
        Tk2_l = Tk2_l + k1[0, :]
        Tk2_r = Tk2_r + l1[0, :]

        Tb0 = np.array(cv2.triangulatePoints(PL, PR, Tb0_l, Tb0_r)).T
        wp_Tb0 = Tb0[0][:-1] / np.max(Tb0[0][-1])
        Tb1 = np.array(cv2.triangulatePoints(PL, PR, Tb1_l, Tb1_r)).T
        wp_Tb1 = Tb1[0][:-1] / np.max(Tb1[0][-1])
        Tb2 = np.array(cv2.triangulatePoints(PL, PR, Tb2_l, Tb2_r)).T
        wp_Tb2 = Tb2[0][:-1] / np.max(Tb2[0][-1])
        Tk0 = np.array(cv2.triangulatePoints(PL, PR, Tk0_l, Tk0_r)).T
        wp_Tk0 = Tk0[0][:-1] / np.max(Tk0[0][-1])
        Tk1 = np.array(cv2.triangulatePoints(PL, PR, Tk1_l, Tk1_r)).T
        wp_Tk1 = Tk1[0][:-1] / np.max(Tk1[0][-1])
        Tk2 = np.array(cv2.triangulatePoints(PL, PR, Tk2_l, Tk2_r)).T
        wp_Tk2 = Tk2[0][:-1] / np.max(Tk2[0][-1])

        # 光电标靶坐标系与世界坐标系间旋转矩阵
        Rw_tb = calPoseFrom3Points(wp_Tb0, wp_Tb1, wp_Tb2)[0]  # 基准标靶
        Rw_tk = calPoseFrom3Points(wp_Tk0, wp_Tk1, wp_Tk2)[0]  # 待测标靶

        Rtb_tk = np.dot(np.transpose(Rw_tb), Rw_tk)  # 待校准标靶系与基准标靶系间旋转矩阵

        angle_hz = 87.06294733
        angle_v = 25.78054998
        hz_b = 74.87478168
        v_b = 40.70786725

        #误差项3.光电标靶与转接架轴线标定误差
        angle_hz = 87.06294733 + hz1
        angle_v = 25.78054998 + v1
        hz_b = 74.87478168 + hz2
        v_b = 40.70786725 + v2

        PP = calPointFrom2Angles(angle_hz, angle_v)
        P = np.dot(PP, Rtb_tk.T).T

        x = P[0]
        y = P[1]
        z = P[2]

        hz_new = math.atan2(-z, x) * 180 / math.pi
        v_new = math.atan2(y, math.sqrt(x ** 2 + z ** 2)) * 180 / math.pi

        hz = hz_b - hz_new
        v = v_b - v_new

        a4list.append(hz)
        b4list.append(v)

    print('a4', a2list)
    print('b4', b2list)


################################### 数据处理 ###############################

################### 水平角 #########################

# 无误差
print('a0max1', round(max(a0list), 4))
print('a0min1', round(min(a0list), 4))
# print('a0ppm1', round(max(a0list) - min(a0list), 4))
print('a0mean1', round(np.mean(a0list), 4))
print('a0std1', round(np.std(a0list, ddof=1), 4))

# Error 1
print('a1')
print(round(max(a1list), 4))
print(round(min(a1list), 4))
# print(round(max(a1list) - min(a1list), 4))
print(round(np.mean(a1list), 4))
print(round(np.std(a1list, ddof=1), 4))

# Error 2
print('a2')
print(round(max(a2list), 4))
print(round(min(a2list), 4))
# print(round(max(a2list) - min(a2list), 4))
print(round(np.mean(a2list), 4))
print(round(np.std(a2list, ddof=1), 4))

# Error 3
print('a3')
print(round(max(a3list), 4))
print(round(min(a3list), 4))
# print(round(max(a3list) - min(a3list), 4))
print(round(np.mean(a3list), 4))
print(round(np.std(a3list, ddof=1), 4))

# Error 4
print('a4')
print( round(max(a4list), 4))
print( round(min(a4list), 4))
# print( round(max(a4list) - min(a4list), 4))
print( round(np.mean(a4list), 4))
print( round(np.std(a4list, ddof=1), 4))


################### 垂直角 #########################

# 无误差
print('b0')
print(round(max(b0list), 4))
print(round(min(b0list), 4))
# print(round(max(b0list) - min(b0list), 4))
print(round(np.mean(b0list), 4))
print(round(np.std(b0list, ddof=1), 4))

# Error 1
print('b1')
print(round(max(b1list), 4))
print(round(min(b1list), 4))
# print(round(max(b1list) - min(b1list), 4))
print(round(np.mean(b1list), 4))
print(round(np.std(b1list, ddof=1), 4))

# Error 2
print('b2')
print(round(max(b2list), 4))
print(round(min(b2list), 4))
# print(round(max(b2list) - min(b2list), 4))
print(round(np.mean(b2list), 4))
print(round(np.std(b2list, ddof=1), 4))

# Error 3
print('b3')
print(round(max(b3list), 4))
print(round(min(b3list), 4))
# print(round(max(b3list) - min(b3list), 4))
print(round(np.mean(b3list), 4))
print(round(np.std(b3list, ddof=1), 4))

# Error 4
print('b4')
print( round(max(b4list), 4))
print( round(min(b4list), 4))
# print( round(max(b4list) - min(b4list), 4))
print( round(np.mean(b4list), 4))
print( round(np.std(b4list, ddof=1), 4))


################################### 画图  ################################

fig = plt.figure()
plt.suptitle('Total error')
ax1 = fig.add_subplot(3, 1, 1)
ax2 = fig.add_subplot(3, 1, 2)

# 设置字体大小
matplotlib.rcParams.update({'font.size': 10})

x = np.linspace(0, 500, 500)

z10 = a0list
z11 = a1list
z12 = a2list
z13 = a3list
z14 = a4list

line10, = ax1.plot(x, z10, linewidth='1.5', color='k')
line11, = ax1.plot(x, z11, linewidth='1', color='c')
line12, = ax1.plot(x, z12, linewidth='1', color='r')
line13, = ax1.plot(x, z13, linewidth='1', color='b')
line14, = ax1.plot(x, z14, linewidth='1', color='m')

ax1.set_xlabel('Point/pcs', fontsize=13)
ax1.set_ylabel('Horizontal angle/°', fontsize=13)

z20 = b0list
z21 = b1list
z22 = b2list
z23 = b3list
z24 = b4list

line20, = ax2.plot(x, z20, linewidth='1.5', color='k', label='Without \n error')
line21, = ax2.plot(x, z21, linewidth='1', color='c', label='Camera \n calibration error')
line22, = ax2.plot(x, z22, linewidth='1', color='r', label='Pixel \n Coordinates')
line23, = ax2.plot(x, z23, linewidth='1', color='b', label='calibration \n error')
line24, = ax2.plot(x, z24, linewidth='1', color='m', label='All error')

ax2.set_xlabel('Point/pcs', fontsize=13)
ax2.set_ylabel('Vertical angle/°', fontsize=13)

ax2.legend(bbox_to_anchor=(1, 0), loc=3, labelspacing=1, fontsize=11)

plt.show()